/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.ortn.wildbots.subsystems;

import edu.ortn.wildbots.commands.MainCommandGroup;
import edu.ortn.wildbots.RobotMap;
import edu.ortn.wildbots.commands.Operate;
import edu.ortn.wildbots.commands.CameraControl;
import edu.ortn.wildbots.sensors.UltrasonicSensor;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
 *
 * @author Stephen
 */
public class Chassis extends Subsystem {

    private RobotDrive drive;
    private UltrasonicSensor ultrasonic;

    private double leftValue;
    private double rightValue;
    private double ultrasonicValue;
    
    public void initDefaultCommand() {
        setDefaultCommand(new MainCommandGroup()); // set default command
    }

    public Chassis() {
        //drive = new RobotDrive(1, 2);
        drive = new RobotDrive(RobotMap.LEFT_MOTOR_PORT, RobotMap.RIGHT_MOTOR_PORT);

        drive.setSafetyEnabled(true);
        
        ultrasonic = new UltrasonicSensor();     
    }
    
    public double getDistanceInInches() {
        this.ultrasonicValue = ultrasonic.getRangeInches();
        return ultrasonicValue;
    }

    public void straight() { // sets the motor speeds to drive straight (no turn)
        drive.arcadeDrive(1.0, 0.0);
    }

    public void turnLeft() { // sets the motor speeds to start a left turn
        drive.arcadeDrive(0.0, 1.0);
    }

    public void arcadeDrive(double y, double turn) {
        drive.arcadeDrive(y, turn);
                
    }

    public void tankDrive(double leftStick, double rightStick) {
        //System.out.println("tankDrive() called: leftStick = " + leftStick + 
        //        "Rigth Stick = " + rightStick);    
        this.leftValue = leftStick;
        this.rightValue = rightStick;
        drive.tankDrive(leftStick, rightStick);           
    }
    //public void setSpeed(double speedMult){
    //    this.speedMult = speedMult;
    //}
    
    public String[] getData() {
        String[] data = new String[3];
        data[0] = "Left Joystick: " + leftValue;
        data[1] = "Right Joystick: " + rightValue;
        data[2] = "Ultrasonic: " + ultrasonic.getRangeInches();
        return data;
    }
}
